#!/usr/bin/env roseus
(load "package://pr2eus_openrave/pr2eus-openrave.l")
(setq *exit-on-fail* nil)

(defun init-settings ()
  (unless (boundp '*pr2*) (pr2))
  (unless (boundp '*ri*)
    (setq *ri* (instance pr2-interface :init)))
  (unless (and x::*display* (> x::*display* 0))
      (objects (list *pr2*)))
  (pr2-tuckarm-pose :larm)
  (ros::subscribe "ray_coords" geometry_msgs::PoseStamped #'pose-cb)
  (ros::subscribe "change_pose_pr2" std_msgs::String #'change-cb)
;;  (setq *begin* (ros::time-now))
;;  (setq *lock* nil)
  )

(defun pose-cb (msg)
  (let ((box (make-cube 100 50 50))
	(htime (send msg :header :stamp))
	(p (ros::tf-pose->coords (send msg :pose)))
	ret)
    ;; (if (or *lock* (> 0 (send (ros::time- htime *begin*) :to-sec)))
    ;; 	(progn
    ;; 	  (ros::ros-warn "return-from pose-cb because of lock or time problem")
    ;; 	  (return-from pose-cb nil)))
;;    (setq *lock* t)
    (dotimes (i 500)
      (remove-marker i))
    (send *ri* :publish-joint-state)
    (ros::ros-info "subscribed~A" p)
    (send box :transform p)
    (unless (and x::*display* (> x::*display* 0))
      (objects (list *pr2* box)))
    (send *pr2* :angle-vector (send *ri* :state :potentio-vector))
    (send *pr2* :head :look-at (send (send p :copy-worldcoords) :worldpos))
    (send *ri* :angle-vector (send *pr2* :angle-vector))
    (setq ret (send *ri* :move-end-coords-plan
		    (send p :copy-worldcoords) :move-arm :rarm :use-torso t :lifetime 10))
    (ros::ros-info "move-end-coords-plan -> ~A~%" ret)
    (if (and *exit-on-fail* (null ret)) (exit 1))
    (send *ri* :wait-interpolation)
    (send *ri* :publish-joint-state)
;;    (setq *lock* nil)
    ))

(defun change-cb(msg)
  (let ((data (read-from-string (send msg :data))))
    (case data
      ('stop-visualize
       (ros::ros-warn "remove-marker")
       (dotimes (i 500)
	 (remove-marker i)))
      ('reset-pose
       (ros::ros-warn "reset-pose")
       (send *pr2* :reset-pose)
       (send *ri* :angle-vector (send *pr2* :angle-vector)))
      ('tuck-arm-r
       (ros::ros-warn "tuckarm-pose")
       (pr2-tuckarm-pose :rarm)
       (send *ri* :publish-joint-state))
      ('tuck-arm-l
       (ros::ros-warn "tuckarm-pose")
       (pr2-tuckarm-pose :larm)
       (send *ri* :publish-joint-state))
      )
    ))

(defun execute-main ()
  (init-settings)
  (ros::rate 10)
  (while (ros::ok)
    (ros::spin-once)
    (if (and x::*display* (> x::*display* 0)) (x::window-main-one))
    (send *ri* :publish-joint-state)
    (ros::sleep)
    ))

